#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "clawcmd_msg/msg/claw_cmd.hpp"
#include <Windows.h>

#include "bdaqctrl.h"
using namespace Automation::BDaq;

using std::placeholders::_1;

static InstantDoCtrl* s_instantDoCtrl = nullptr;
static HMODULE instance = LoadLibrary(TEXT("biodaq.dll"));


class RobotClaw : public rclcpp::Node
{
public:
    RobotClaw()
    : Node("robotclaw_node")
    {
        subscription_ = this->create_subscription<clawcmd_msg::msg::ClawCmd>("ClawCmd", 10, std::bind(&RobotClaw::topic_callback, this, _1));

        s_instantDoCtrl = InstantDoCtrl::Create();
        s_instantDoCtrl->setSelectedDevice(DeviceInformation(L"PCI-1730,BID#0"));
    }

private:
    void topic_callback(const clawcmd_msg::msg::ClawCmd::SharedPtr msg) const
    {
        RCLCPP_INFO(this->get_logger(), "I heard claw_dir: '%d'", msg->claw_dir);
        RCLCPP_INFO(this->get_logger(), "I heard claw_force: '%d'", msg->claw_force);

        // // claw_dir is number 0 claw actually
        // // claw_force is number 1 claw actually

        // if (msg->claw_dir < 6)
        // {
        //     s_instantDoCtrl->Write(0, msg->claw_dir);
        // }

        if (msg->claw_force < 6)
        {
            s_instantDoCtrl->Write(msg->claw_dir, msg->claw_force);
        }
    }

    rclcpp::Subscription<clawcmd_msg::msg::ClawCmd>::SharedPtr subscription_;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<RobotClaw>());
    rclcpp::shutdown();
    return 0;
}